//
// Created by augtons on 2022/1/10.
//

#ifndef WINDSWING_WINDSWING_SPECIFIC_H
#define WINDSWING_WINDSWING_SPECIFIC_H

#include "freertos/FreeRTOS.h"
#include "moto_ctrl.h"
#include "imu.h"

#define BEHAVIOR_TASK_S (2048)
#define BEHAVIOR_TASK_P (4)

float swing_len;
float swing_M;
float swing_N;
float swing_theta1;
float swing_theta2;


struct WindSwingStatus{
    spinlock_t spinlock;
    float tileAngle;
    float azimuth;
    EulerAngle eulerAngle;
}windSwingStatus;

enum WindSwingBehavior{
    BEHAVIOR_RESET = 0,
    BEHAVIOR_SINGLE_MOTO,
    BEHAVIOR_LEFT_RIGHT_SWING,
    BEHAVIOR_ANGLE_SWING,
    BEHAVIOR_SHUTDOWN,
    BEHAVIOR_DRAW_CIRCLE,
}windSwingBehavior;

int behavior_data;  //such as angle or radium

struct motoPwm{
    float l;    //line
    float r;    //radium
}motoPwm;

esp_timer_handle_t windswingTimer;
TaskHandle_t currentTask;
TaskHandle_t currentTask2;

void initWindSwing();
void set_tileAngle(float tileAngle);
void set_azimuth(float azimuth);
void set_eularAngle(EulerAngle *eulerAngle);
void get_eularAngle(EulerAngle *eulerAngle);

float get_tileAngle();
float get_azimuth();

_Noreturn void task_line(void *args);
_Noreturn void task_stop_self(void *args);
_Noreturn void task_circle(void *args);

#endif //WINDSWING_WINDSWING_SPECIFIC_H
